#!/usr/bin/env python3
# -*- coding:utf-8 -*-      

import rospy 
import numpy as np
import datetime
from serial_ground.msg import Cmd_send, Uav_state, FWstates, Leaderstates

STATE_UAV = 101
CMD_UAV = 102

STATE_UAV = 101
CMD_UAV = 102
ARM = 1
TAKEOFF = 2
LAND = 3
POS = 4
VEL = 5
ACC = 6
POS_VEL = 7
POS_ACC = 8
VEL_ACC = 9
POS_VEL_ACC = 10
ATT = 11

outdoor_uav_cmd = Cmd_send()

leader_id = "plane_0"
fw_id = "plane_1"
    
def vir_leader_cb(msg:Leaderstates):
    global outdoor_uav_cmd
    outdoor_uav_cmd.exp_local_x = msg.ned_pos_x
    outdoor_uav_cmd.exp_local_y = msg.ned_pos_y
    outdoor_uav_cmd.exp_local_z = msg.ned_pos_z
    outdoor_uav_cmd.exp_local_vx = msg.ned_vel_x
    outdoor_uav_cmd.exp_local_vy = msg.ned_vel_y
    outdoor_uav_cmd.exp_local_vz = msg.ned_vel_z
    outdoor_uav_cmd.cmd_type = msg.cmd_type
    
    time = datetime.datetime.now()
    hour = time.hour
    minute = time.minute
    second = time.second
    
    # print(hour, minute, second)
    
    outdoor_uav_cmd.hour = hour
    outdoor_uav_cmd.minutes = minute
    outdoor_uav_cmd.seconds= second

def cmd_out():
	global outdoor_uav_cmd
	try:
		# Try to publish /outdoor/uav/cmd
		outdoor_uav_cmd.frame_id = CMD_UAV
		outdoor_uav_cmd_pub.publish(outdoor_uav_cmd)

	except Exception as e:
		print(e)

###########################################################################################
# main process
if __name__ == '__main__':
	rospy.init_node('PX4_fixedwing_control_center')
	rospy.loginfo("GCS node has been started!")
	
	vir_leader_sub = rospy.Subscriber(leader_id + "/fixed_wing_control/fw_state", Leaderstates, vir_leader_cb, queue_size=1)
	outdoor_uav_cmd_pub = rospy.Publisher('/outdoor/uav/cmd', Cmd_send, queue_size=1)

	rate = rospy.Rate(20)
	while not rospy.is_shutdown():
		cmd_out()
		rate.sleep()
